/**
 * MUL8.cpp
 *
 *  Created on: Nov 13, 2013
 *      Author: Kellen Carey
 *
 *  Edited 12/30/2013 by Adam Stroud
 *  Added Instance of BatteryMonitor
 */

#include "MUL8.h"
#include "MotorController.h"
#include "Vision.h"
#include "Android_Control.h"
//#include "InertialData.h"
//#include "BatteryMonitor.h"
#include <stdio.h>
#include <time.h>

MUL8::MUL8() {


}

MUL8::~MUL8() {

}

void MUL8::run() {
	// Create an instance of MotorController, with which we can perform different motions

	MotorController motorController;
	Android_Control android_control;
	Vision vision;

/* this is for testing. without it, mul8 will remain limp until a command is issued*/
	motorController.setMotion(STAND);
	//motorController.executeNextStep(motorController.getMotionFile());
	//motorController.executeNextStep(motorController.getMotionFile());
	//std::string useless;
	//std::cin >> useless;
/*********************************/





	// Create an instance of InertialData, which will check to see if the robot is currently falling
	//	InertialData inertialData; // TODO Get this class to work

	// Create an instance of BatteryMonitor, which will check to see if the robot has healthy battery levels
	//BatteryMonitor batteryMonitor; // TODO Get this class to work

	// This variable will change every time the vision instance steps
	int movement = 0;
	float voltage = 0;
	int command = 0;
	int visionOccupier;
	String commandName;
	bool visionOFF= true;
	bool androidON= true;

	// Step once with vision, should begin looking for the ball
	//movement = vision.step(false /*inertialData.isFalling()false*/);
	//voltage = batteryMonitor.step(false);
	// Infinite loop
	while (true)
	{
		// The following false's will eventually be replaced by inertialData.step()
		// Keep trying to find the ball, I believe each step analyzes one image frame
		//movement = vision.step(false /*inertialData.isFalling()*/);
		//voltage = batteryMonitor.step(false);
		command= 0; //default for android= nothing was said
		visionOccupier= vision.step(visionOFF);// vision only runs when visionOFF= false
		std::cout<<"result of vision: "<<visionOccupier<<"\n"<<std::endl;


		//std::cout << voltage << std::endl;
		//std::cout << movement << std::endl;

		/**
		* comment out the following block if you wish to test without Android control
		*/
		command = android_control.step(androidON);

		if(command!=0){
			/* this switch case deals with the results from android communication*/
			/**
			 * 0=stand
			 * 1=walk
			 * 2=turnleft
			 * 3=turnright
			 * 4=sidestepleft
			 * 5=sidestepright
			 * 6=kick
			 */

			if(command==1){

				commandName= "Stand";

				std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

				motorController.setMotion(STAND);// We need to update the current motion from whatever vision.step() has returned

				clock_t startTime = clock();

				while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 7) {
					motorController.step(false);
				}

			}
			if(command==2){

				commandName= "Walk";

				std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

				motorController.setMotion(WALK);// We need to update the current motion from whatever vision.step() has returned

				clock_t startTime = clock();

				while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 7) {
					motorController.step(false);
				}
			}
			if(command==3){

				commandName= "Kick";

				std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

				motorController.setMotion(KICK);// We need to update the current motion from whatever vision.step() has returned

				clock_t startTime = clock();

				while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 7) {
						motorController.step(false);
				}

			}
			if(command==4){

				commandName= "relax";

				std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

				motorController.disableAllMotors();// We need to update the current motion from whatever vision.step() has returned
				}


			if(command==5){

				commandName= "enable";

				std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

				motorController.enableAllMotors();// We need to update the current motion from whatever vision.step() has returned
				}


			/* When 'Track' is recieved from android, vision.step begins begins*/
			else if(command==6){
				commandName= "Track Ball";
				std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;
				visionOFF= false;
				androidON=false;

			}

		}




		/**
		 * comment the following 6 lines out if you wish to test without full body motions
		 */



	}

	// Should never get here
	return;



}
